#include "utility.hpp"
#include "lio_sam/msg/cloud_info.hpp"

// Boost (用于 optional)
#include <boost/optional.hpp>

// PCL (用于RANSAC和点云操作)
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/filters/extract_indices.h>


// ===================================================================================
// ========================== 传感器点云结构体定义 (保持不变) ==========================
// ===================================================================================
struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(VelodynePointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time))

struct OusterPointXYZIRT
{
    PCL_ADD_POINT4D;
    float intensity;
    uint32_t t;
    uint16_t reflectivity;
    uint8_t ring;
    uint16_t noise;
    uint32_t range;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint32_t, t, t)(uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring)(uint16_t, noise, noise)(uint32_t, range, range))

using PointXYZIRT = VelodynePointXYZIRT;
const int queueLength = 2000;


// ===================================================================================
// ================================ ImageProjection 类定义 ============================
// ===================================================================================
class ImageProjection : public ParamServer
{
private:
    // 锁
    std::mutex imuLock;
    std::mutex odoLock;

    // ROS 订阅与发布
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subLaserCloud;
    rclcpp::CallbackGroup::SharedPtr callbackGroupLidar;

    rclcpp::Publisher<lio_sam::msg::CloudInfo>::SharedPtr pubLaserCloudInfo;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubExtractedCloud;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubIntensityCloud;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubGroundCloud;

    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subImu;
    rclcpp::CallbackGroup::SharedPtr callbackGroupImu;

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subOdom;
    rclcpp::CallbackGroup::SharedPtr callbackGroupOdom;

    // 数据队列
    std::deque<sensor_msgs::msg::Imu> imuQueue;
    std::deque<nav_msgs::msg::Odometry> odomQueue;
    std::deque<sensor_msgs::msg::PointCloud2> cloudQueue;
    
    // 消息和头文件
    sensor_msgs::msg::PointCloud2 currentCloudMsg;
    std_msgs::msg::Header cloudHeader;
    lio_sam::msg::CloudInfo cloudInfo;

    // IMU 预积分/去畸变相关
    std::vector<double> imuTime, imuRotX, imuRotY, imuRotZ;
    int imuPointerCur;
    bool firstPointFlag;
    Eigen::Affine3f transStartInverse;

    // 点云指针
    pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
    pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
    pcl::PointCloud<PointType>::Ptr fullCloud;
    pcl::PointCloud<PointType>::Ptr extractedCloud;
    pcl::PointCloud<PointType>::Ptr intensityCloud;
    pcl::PointCloud<PointType>::Ptr groundCloud;

    // 图像投影/参数
    int ringFlag;
    int deskewFlag;
    cv::Mat rangeMat;
    float intensityThreshold;

    // 里程计去畸变
    bool odomDeskewFlag;
    float odomIncreX;
    float odomIncreY;
    float odomIncreZ;
    
    // 时间戳
    double timeScanCur;
    double timeScanEnd;
    
    vector<int> columnIdnCountVec;

    // 地面检测参数
    double sensor_height;
    double ground_height_clip_range;
    int ground_pts_thresh;
    double ground_normal_thresh;

    // =================================================
    // ============= 成员函数声明 =======================
    // =================================================
    void allocateMemory();
    void resetParameters();
    void imuHandler(const sensor_msgs::msg::Imu::SharedPtr imuMsg);
    void odometryHandler(const nav_msgs::msg::Odometry::SharedPtr odometryMsg);
    void cloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg);
    bool cachePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr& laserCloudMsg);
    bool deskewInfo();
    void imuDeskewInfo();
    void odomDeskewInfo();
    void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur);
    void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur);
    PointType deskewPoint(PointType *point, double relTime);
    void projectPointCloud();
    void cloudExtraction();
    void publishClouds();
    
    // 新增的地面检测函数声明
    boost::optional<Eigen::Vector4f> detect_floor_plane(
        const pcl::PointCloud<PointType>::Ptr& cloud_in,
        pcl::PointCloud<PointType>::Ptr& ground_points_out);

public:
    ImageProjection(const rclcpp::NodeOptions &options);
    ~ImageProjection() {}
};


// ===================================================================================
// ============================== ImageProjection 类函数实现 ===========================
// ===================================================================================

ImageProjection::ImageProjection(const rclcpp::NodeOptions &options) : ParamServer("lio_sam_imageProjection", options), deskewFlag(0)
{
    // 回调组
    callbackGroupLidar = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callbackGroupImu = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callbackGroupOdom = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    auto lidarOpt = rclcpp::SubscriptionOptions();
    lidarOpt.callback_group = callbackGroupLidar;
    auto imuOpt = rclcpp::SubscriptionOptions();
    imuOpt.callback_group = callbackGroupImu;
    auto odomOpt = rclcpp::SubscriptionOptions();
    odomOpt.callback_group = callbackGroupOdom;

    // 订阅者
    subImu = create_subscription<sensor_msgs::msg::Imu>(
        imuTopic, qos_imu, std::bind(&ImageProjection::imuHandler, this, std::placeholders::_1), imuOpt);
    subOdom = create_subscription<nav_msgs::msg::Odometry>(
        odomTopic + "_incremental", qos_imu, std::bind(&ImageProjection::odometryHandler, this, std::placeholders::_1), odomOpt);
    subLaserCloud = create_subscription<sensor_msgs::msg::PointCloud2>(
        pointCloudTopic, qos_lidar, std::bind(&ImageProjection::cloudHandler, this, std::placeholders::_1), lidarOpt);

    // 发布者
    pubExtractedCloud = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/deskew/cloud_deskewed", 1);
    pubIntensityCloud = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/deskew/cloud_intensity", 1);
    pubGroundCloud = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/deskew/cloud_ground", 1);
    pubLaserCloudInfo = create_publisher<lio_sam::msg::CloudInfo>("lio_sam/deskew/cloud_info", qos);

    // 加载参数
    intensityThreshold = declare_parameter<float>("intensity_threshold", 150.0);
    sensor_height = declare_parameter<double>("sensor_height", 1.5);
    ground_height_clip_range = declare_parameter<double>("ground_height_clip_range", 0.3);
    ground_pts_thresh = declare_parameter<int>("ground_pts_thresh", 512);
    ground_normal_thresh = declare_parameter<double>("ground_normal_thresh", 10.0);
    
    // 初始化
    allocateMemory();
    resetParameters();
    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}

void ImageProjection::allocateMemory()
{
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
    tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
    fullCloud.reset(new pcl::PointCloud<PointType>());
    extractedCloud.reset(new pcl::PointCloud<PointType>());
    intensityCloud.reset(new pcl::PointCloud<PointType>());
    groundCloud.reset(new pcl::PointCloud<PointType>());

    fullCloud->points.resize(N_SCAN * Horizon_SCAN);

    cloudInfo.start_ring_index.assign(N_SCAN, 0);
    cloudInfo.end_ring_index.assign(N_SCAN, 0);
    cloudInfo.point_col_ind.assign(N_SCAN * Horizon_SCAN, 0);
    cloudInfo.point_range.assign(N_SCAN * Horizon_SCAN, 0);

    imuTime.assign(queueLength, 0.0);
    imuRotX.assign(queueLength, 0.0);
    imuRotY.assign(queueLength, 0.0);
    imuRotZ.assign(queueLength, 0.0);

    resetParameters();
}

void ImageProjection::resetParameters()
{
    laserCloudIn->clear();
    extractedCloud->clear();
    intensityCloud->clear();
    groundCloud->clear();
    rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));

    imuPointerCur = 0;
    firstPointFlag = true;
    odomDeskewFlag = false;

    for (int i = 0; i < queueLength; ++i) {
        imuTime[i] = 0;
        imuRotX[i] = 0;
        imuRotY[i] = 0;
        imuRotZ[i] = 0;
    }
    columnIdnCountVec.assign(N_SCAN, 0);
}

void ImageProjection::imuHandler(const sensor_msgs::msg::Imu::SharedPtr imuMsg)
{
    sensor_msgs::msg::Imu thisImu = imuConverter(*imuMsg);
    std::lock_guard<std::mutex> lock1(imuLock);
    imuQueue.push_back(thisImu);
}

void ImageProjection::odometryHandler(const nav_msgs::msg::Odometry::SharedPtr odometryMsg)
{
    std::lock_guard<std::mutex> lock2(odoLock);
    odomQueue.push_back(*odometryMsg);
}

void ImageProjection::cloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg)
{
    if (!cachePointCloud(laserCloudMsg)) return;
    if (!deskewInfo()) return;
    
    projectPointCloud();

    auto floor_coeffs_optional = detect_floor_plane(fullCloud, groundCloud);

    if (floor_coeffs_optional) {
        cloudInfo.floor_available = true;
        for (int i = 0; i < 4; ++i) {
            cloudInfo.floor_coeffs[i] = (*floor_coeffs_optional)[i];
        }
    } else {
        cloudInfo.floor_available = false;
        for (int i = 0; i < 4; ++i) {
            cloudInfo.floor_coeffs[i] = 0;
        }
    }

    cloudExtraction();
    publishClouds();
    resetParameters();
}

// 新增的地面检测函数实现
boost::optional<Eigen::Vector4f> ImageProjection::detect_floor_plane(
    const pcl::PointCloud<PointType>::Ptr& cloud_in, 
    pcl::PointCloud<PointType>::Ptr& ground_points_out)
{
    ground_points_out->clear();

    pcl::PointCloud<PointType>::Ptr filtered_cloud(new pcl::PointCloud<PointType>());
    filtered_cloud->reserve(cloud_in->size());

    for (const auto& pt : cloud_in->points) {
        if (pt.z > -sensor_height - ground_height_clip_range && pt.z < -sensor_height + ground_height_clip_range) {
            filtered_cloud->push_back(pt);
        }
    }

    // RCLCPP_INFO(get_logger(), "[Ground Detect] Points after height filtering: %ld", filtered_cloud->size());

    if (filtered_cloud->size() < (size_t)ground_pts_thresh) {
        RCLCPP_WARN(get_logger(), "Not enough points for ground detection after height filtering. Points: %ld", filtered_cloud->size());
        return boost::none;
    }

    pcl::SampleConsensusModelPlane<PointType>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointType>(filtered_cloud));
    pcl::RandomSampleConsensus<PointType> ransac(model_p);
    ransac.setDistanceThreshold(0.1);
    ransac.computeModel();

    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    ransac.getInliers(inliers->indices);

    // RCLCPP_INFO(get_logger(), "[Ground Detect] RANSAC inliers: %ld", inliers->indices.size());

    if (inliers->indices.size() < (size_t)ground_pts_thresh) {
        RCLCPP_WARN(get_logger(), "RANSAC found a plane, but with too few inliers: %ld", inliers->indices.size());
        return boost::none;
    }

    Eigen::VectorXf coeffs;
    ransac.getModelCoefficients(coeffs);

    double dot = coeffs.head<3>().dot(Eigen::Vector3f::UnitZ());
    if (std::abs(dot) < std::cos(ground_normal_thresh * M_PI / 180.0)) {
        RCLCPP_WARN(get_logger(), "Detected plane's normal is not vertical enough. Dot product: %f", dot);
        return boost::none;
    }

    if (coeffs(2) < 0) {
        coeffs *= -1.0;
    }

    pcl::ExtractIndices<PointType> extract;
    extract.setInputCloud(filtered_cloud);
    extract.setIndices(inliers);
    extract.filter(*ground_points_out);

    // RCLCPP_INFO(get_logger(), "[Ground Detect] Plane normal dot Z-axis: %f", dot);
    // RCLCPP_INFO(get_logger(), "[Ground Detect] Final Plane Coeffs: [%f, %f, %f, %f]", coeffs[0], coeffs[1], coeffs[2], coeffs[3]);

    return Eigen::Vector4f(coeffs);
}


// ===================================================================================
// ========================== 以下是LIO-SAM原有函数 (保持不变) ========================
// ===================================================================================

bool ImageProjection::cachePointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr &laserCloudMsg)
{
    cloudQueue.push_back(*laserCloudMsg);
    if (cloudQueue.size() <= 2)
        return false;

    currentCloudMsg = std::move(cloudQueue.front());
    cloudQueue.pop_front();
    if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)
    {
        pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
    }
    else if (sensor == SensorType::OUSTER)
    {
        pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
        laserCloudIn->points.resize(tmpOusterCloudIn->size());
        laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
        for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
        {
            auto &src = tmpOusterCloudIn->points[i];
            auto &dst = laserCloudIn->points[i];
            dst.x = src.x;
            dst.y = src.y;
            dst.z = src.z;
            dst.intensity = src.intensity;
            dst.ring = src.ring;
            dst.time = src.t * 1e-9f;
        }
    }
    else
    {
        RCLCPP_ERROR_STREAM(get_logger(), "Unknown sensor type: " << int(sensor));
        rclcpp::shutdown();
    }

    cloudHeader = currentCloudMsg.header;
    timeScanCur = stamp2Sec(cloudHeader.stamp);
    timeScanEnd = timeScanCur + laserCloudIn->points.back().time;

    vector<int> indices;
    pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

    if (laserCloudIn->is_dense == false)
    {
        RCLCPP_ERROR(get_logger(), "Point cloud is not in dense format, please remove NaN points first!");
        rclcpp::shutdown();
    }

    if (ringFlag == 0)
    {
        ringFlag = -1;
        for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
        {
            if (currentCloudMsg.fields[i].name == "ring")
            {
                ringFlag = 1;
                break;
            }
        }
        if (ringFlag == -1)
        {
            if (sensor == SensorType::VELODYNE)
            {
                ringFlag = 2;
            }
            else
            {
                RCLCPP_ERROR(get_logger(), "Point cloud ring channel not available, please configure your point cloud data!");
                rclcpp::shutdown();
            }
        }
    }

    if (deskewFlag == 0)
    {
        deskewFlag = -1;
        for (auto &field : currentCloudMsg.fields)
        {
            if (field.name == "time" || field.name == "t" || field.name == "timestamp")
            {
                deskewFlag = 1;
                break;
            }
        }
        if (deskewFlag == -1)
            RCLCPP_WARN(get_logger(), "Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
    }

    return true;
}

bool ImageProjection::deskewInfo()
{
    std::lock_guard<std::mutex> lock1(imuLock);
    std::lock_guard<std::mutex> lock2(odoLock);

    if (imuQueue.empty() ||
        stamp2Sec(imuQueue.front().header.stamp) > timeScanCur ||
        stamp2Sec(imuQueue.back().header.stamp) < timeScanEnd)
    {
        RCLCPP_INFO(get_logger(), "Waiting for IMU data ...");
        return false;
    }

    imuDeskewInfo();
    odomDeskewInfo();
    return true;
}

void ImageProjection::imuDeskewInfo()
{
    cloudInfo.imu_available = false;
    while (!imuQueue.empty())
    {
        if (stamp2Sec(imuQueue.front().header.stamp) < timeScanCur - 0.01)
            imuQueue.pop_front();
        else
            break;
    }

    if (imuQueue.empty()) return;

    imuPointerCur = 0;
    for (int i = 0; i < (int)imuQueue.size(); ++i)
    {
        sensor_msgs::msg::Imu thisImuMsg = imuQueue[i];
        double currentImuTime = stamp2Sec(thisImuMsg.header.stamp);

        if (currentImuTime <= timeScanCur)
            imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imu_roll_init, &cloudInfo.imu_pitch_init, &cloudInfo.imu_yaw_init);
        if (currentImuTime > timeScanEnd + 0.01)
            break;

        if (imuPointerCur == 0)
        {
            imuRotX[0] = 0;
            imuRotY[0] = 0;
            imuRotZ[0] = 0;
            imuTime[0] = currentImuTime;
            ++imuPointerCur;
            continue;
        }

        double angular_x, angular_y, angular_z;
        imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

        double timeDiff = currentImuTime - imuTime[imuPointerCur - 1];
        imuRotX[imuPointerCur] = imuRotX[imuPointerCur - 1] + angular_x * timeDiff;
        imuRotY[imuPointerCur] = imuRotY[imuPointerCur - 1] + angular_y * timeDiff;
        imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur - 1] + angular_z * timeDiff;
        imuTime[imuPointerCur] = currentImuTime;
        ++imuPointerCur;
    }

    --imuPointerCur;
    if (imuPointerCur <= 0) return;

    cloudInfo.imu_available = true;
}

void ImageProjection::odomDeskewInfo()
{
    cloudInfo.odom_available = false;
    while (!odomQueue.empty())
    {
        if (stamp2Sec(odomQueue.front().header.stamp) < timeScanCur - 0.01)
            odomQueue.pop_front();
        else
            break;
    }

    if (odomQueue.empty()) return;
    if (stamp2Sec(odomQueue.front().header.stamp) > timeScanCur) return;

    nav_msgs::msg::Odometry startOdomMsg;
    for (int i = 0; i < (int)odomQueue.size(); ++i)
    {
        startOdomMsg = odomQueue[i];
        if (stamp2Sec(startOdomMsg.header.stamp) < timeScanCur)
            continue;
        else
            break;
    }

    tf2::Quaternion orientation;
    tf2::fromMsg(startOdomMsg.pose.pose.orientation, orientation);
    double roll, pitch, yaw;
    tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

    cloudInfo.initial_guess_x = startOdomMsg.pose.pose.position.x;
    cloudInfo.initial_guess_y = startOdomMsg.pose.pose.position.y;
    cloudInfo.initial_guess_z = startOdomMsg.pose.pose.position.z;
    cloudInfo.initial_guess_roll = roll;
    cloudInfo.initial_guess_pitch = pitch;
    cloudInfo.initial_guess_yaw = yaw;
    cloudInfo.odom_available = true;

    odomDeskewFlag = false;
    if (stamp2Sec(odomQueue.back().header.stamp) < timeScanEnd) return;

    nav_msgs::msg::Odometry endOdomMsg;
    for (int i = 0; i < (int)odomQueue.size(); ++i)
    {
        endOdomMsg = odomQueue[i];
        if (stamp2Sec(endOdomMsg.header.stamp) < timeScanEnd)
            continue;
        else
            break;
    }

    if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) return;

    Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
    tf2::fromMsg(endOdomMsg.pose.pose.orientation, orientation);
    tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
    Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
    Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

    float rollIncre, pitchIncre, yawIncre;
    pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
    odomDeskewFlag = true;
}

void ImageProjection::findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
    *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
    int imuPointerFront = 0;
    while (imuPointerFront < imuPointerCur)
    {
        if (pointTime < imuTime[imuPointerFront]) break;
        ++imuPointerFront;
    }
    if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
    {
        *rotXCur = imuRotX[imuPointerFront];
        *rotYCur = imuRotY[imuPointerFront];
        *rotZCur = imuRotZ[imuPointerFront];
    } else {
        int imuPointerBack = imuPointerFront - 1;
        double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
        double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
        *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
        *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
        *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
    }
}

void ImageProjection::findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
    *posXCur = 0; *posYCur = 0; *posZCur = 0;
}

PointType ImageProjection::deskewPoint(PointType *point, double relTime)
{
    if (deskewFlag == -1 || cloudInfo.imu_available == false)
        return *point;

    double pointTime = timeScanCur + relTime;

    float rotXCur, rotYCur, rotZCur;
    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

    float posXCur, posYCur, posZCur;
    findPosition(relTime, &posXCur, &posYCur, &posZCur);

    if (firstPointFlag == true)
    {
        transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
        firstPointFlag = false;
    }

    Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
    Eigen::Affine3f transBt = transStartInverse * transFinal;

    PointType newPoint;
    newPoint.x = transBt(0, 0) * point->x + transBt(0, 1) * point->y + transBt(0, 2) * point->z + transBt(0, 3);
    newPoint.y = transBt(1, 0) * point->x + transBt(1, 1) * point->y + transBt(1, 2) * point->z + transBt(1, 3);
    newPoint.z = transBt(2, 0) * point->x + transBt(2, 1) * point->y + transBt(2, 2) * point->z + transBt(2, 3);
    newPoint.intensity = point->intensity;

    return newPoint;
}

void ImageProjection::projectPointCloud()
{
    int cloudSize = laserCloudIn->points.size();
    for (int i = 0; i < cloudSize; ++i)
    {
        PointType thisPoint;
        thisPoint.x = laserCloudIn->points[i].x;
        thisPoint.y = laserCloudIn->points[i].y;
        thisPoint.z = laserCloudIn->points[i].z;
        thisPoint.intensity = laserCloudIn->points[i].intensity;

        float range = pointDistance(thisPoint);
        if (range < lidarMinRange || range > lidarMaxRange)
            continue;

        int rowIdn = laserCloudIn->points[i].ring;
        if (ringFlag == 2)
        {
            float verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
            rowIdn = (verticalAngle + (N_SCAN - 1)) / 2.0;
        }

        if (rowIdn < 0 || rowIdn >= N_SCAN)
            continue;

        if (rowIdn % downsampleRate != 0)
            continue;

        int columnIdn = -1;
        if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
        {
            float horizonAngle = atan2(thisPoint.y, thisPoint.x) * 180 / M_PI;
            static float ang_res_x = 360.0 / float(Horizon_SCAN);
            columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;
        }
        else if (sensor == SensorType::LIVOX)
        {
            columnIdn = columnIdnCountVec[rowIdn];
            columnIdnCountVec[rowIdn] += 1;
        }

        if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
            continue;

        if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
            continue;

        float originalIntensity = laserCloudIn->points[i].intensity;

        thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

        if (originalIntensity > intensityThreshold)
        {
            intensityCloud->push_back(thisPoint);
        }

        rangeMat.at<float>(rowIdn, columnIdn) = range;
        int index = columnIdn + rowIdn * Horizon_SCAN;
        fullCloud->points[index] = thisPoint;
    }
}

void ImageProjection::cloudExtraction()
{
    int count = 0;
    for (int i = 0; i < N_SCAN; ++i)
    {
        cloudInfo.start_ring_index[i] = count - 1 + 5;
        for (int j = 0; j < Horizon_SCAN; ++j)
        {
            if (rangeMat.at<float>(i, j) != FLT_MAX)
            {
                cloudInfo.point_col_ind[count] = j;
                cloudInfo.point_range[count] = rangeMat.at<float>(i, j);
                extractedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
                ++count;
            }
        }
        cloudInfo.end_ring_index[i] = count - 1 - 5;
    }
}

void ImageProjection::publishClouds()
{
    cloudInfo.header = cloudHeader;
    cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
    cloudInfo.cloud_intensity = publishCloud(pubIntensityCloud, intensityCloud, cloudHeader.stamp, lidarFrame);
    cloudInfo.cloud_ground = publishCloud(pubGroundCloud, groundCloud, cloudHeader.stamp, lidarFrame);
    pubLaserCloudInfo->publish(cloudInfo);
}


// ===================================================================================
// =================================== main 函数 =====================================
// ===================================================================================
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::NodeOptions options;
    options.use_intra_process_comms(true);
    rclcpp::executors::MultiThreadedExecutor exec;
    auto IP = std::make_shared<ImageProjection>(options);
    exec.add_node(IP);
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Image Projection Started.\033[0m");
    exec.spin();
    rclcpp::shutdown();
    return 0;
}